% run_force_control

function [time, force, output, J, controller] = run_force_control(time_profile, force_profile)

phases = 3;
output = cell(phases, 2);

x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, evalin('base','td_angle'), 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];

% controller = ComControllerClass;
controller = ForceControllerClass;

controller.time_profile = time_profile;
controller.force_profile = force_profile;

sim = ForceControlModelClass(x0, @dissipative_environment, controller);

for i = 1 : phases
    [output{i, 1}, output{i, 2}] = sim.run();
end

t = output{2,1};
        x = output{2,2};

r = sqrt( ( x(:,sim.yci) - x(:,sim.yti) ).^2 + ...
    ( x(:,sim.xci) - x(:,sim.xti) ).^2 );

time = output{2,1}-output{2,1}(1);
force = - sim.kl .* r  ...
            + sim.kl .* sim.rl .* x(:,sim.oli) ./ sim.G;

        
        
        
        r = sqrt( ( x(:,sim.yci) - x(:,sim.yti) ).^2 + ...
            ( x(:,sim.xci) - x(:,sim.xti) ).^2 );
        
        power = sim.kl .* r .* sim.rl .* x(:,sim.doli) ./ sim.G ...
            - sim.kl .* sim.rl^2 .* x(:,sim.oli) .* x(:,sim.doli) ./ sim.G^2;
        
        J = trapz(t, abs(power));
        
        
anim = AnimateForceControlModelClass(sim.gnd, output);
anim.animate;

end